
#include "libmri.h"

#include <algorithm>
using std::max;
using std::min;

void kreduce2d (cplxf *KKt, cplxf *K, int ks1, int ks2, int n, int m)
{
	for (int i = 0; i < (2*ks1-1)*(2*ks2-1)*n*n; ++i){ KKt[i] = 0.0f; }

	for (int i = 0; i < n; ++i){
		for (int j = 0; j < n; ++j)
			for (int k = 0; k < m; ++k)
			{
				cplxf *KKtij = &KKt [(i + n*j)*(2*ks1-1)*(2*ks2-1)],
				        *Kik = &K [(i + n*k)*ks1*ks2],
				        *Kjk = &K [(j + n*k)*ks1*ks2];

				for (int i2 = 0; i2 < 2*ks2-1; ++i2)
	 				for (int i1 = 0; i1 < 2*ks1-1; ++i1)
					{
					/*
						for (int j2 = max(0, i2-ks2); j2 <= min(i2, ks2-1); ++j2)
							for (int j1 = max(0, i1-ks1); j1 <= min(i1, ks1-1); ++j1)
								KKtij [i1 + i2*(2*ks1-1)] += 
										Kik [(i1-j1) + (i2-j2)*ks1] *
										* conj (Kjk [j1 + ks1*j2])
					*/
						for (int j2 = 0; j2 < ks2; ++j2)
							for (int j1 = 0; j1 < ks1; ++j1)
							{
								if ((i1-j1 >= 0) and (i1-j1 < ks1)
								and (i2-j2 >= 0) and (i2-j2 < ks2))
									KKtij [i1 + i2*(2*ks1-1)] += 
										Kik [(i1-j1) + (i2-j2)*ks1]
										* conj (Kjk [ks1-1-j1 + (ks2-1-j2)*ks1]);
							}
					}
			}
	}
}

#ifdef _MAIN_

int main (int argc, char **argv)
{
	shm_array_t *shmK = 0;

	struct arg *args [] = {
		new arg_t <shm_array_t*> ("K", shmK),
		0
	};

	get_args (argc, argv, args);

	if (shmK == 0)
		if (get_shm_array ("K", &shmK)){
			perror("KKt");
			return -1;
		}

	cplxf *K = (cplxf*) shmK->data.c;

	int ks1 = shmK->dims[0],
	    ks2 = shmK->dims[1],
	      n = shmK->dims[2],
	      m = shmK->dims[3];

	shm_array_t shmKKt_size, *shmKKt;

	memset (&shmKKt_size, 0, sizeof (shm_array_t));
	shmKKt_size.ndims = 4;
	shmKKt_size.dims[0] = 2*ks1-1;
	shmKKt_size.dims[1] = 2*ks2-1;
	shmKKt_size.dims[2] = n;
	shmKKt_size.dims[3] = n;
	shmKKt_size.type = shm_array_cplxf;

	if (make_shm_array ("KKt", &shmKKt_size, &shmKKt)){
		perror ("make_shm_array(KKt)");
		return -1;
	}

	cplxf *KKt = (cplxf*) shmKKt->data.c;

	kreduce2d (KKt, K, ks1, ks2, n, m);

	put_shm_array ("KKt", shmKKt);
	

	return 0;
}

#endif
